#include "globals.h"

class BugTracker
{
public:
	BugTracker();
	~BugTracker();

	void Init( int numCams, const valarray<float>& initT, const valarray<float>& initR, const valarray<float>& initAng );
	void UpdateEstParams( valarray<float>& T, valarray<float>& R, valarray<float>& Ang );
	void GetBestEstParams( valarray<float>& T, valarray<float>& R, valarray<float>& Ang );
	bool IsConverged() { return bIsConverged; }
	void AddNextRealView( CvMat* inputRealView, int camIdx );
	void AddNextGenView( CvMat* inputGenView, int camIdx );
	bool IsInit() { return bIsInit; }
	void SetTruth( const valarray<float>& T, const valarray<float>& R,
		const valarray<float>& A) {
			truT = T; 
			truR = R; 
			truAng = A;					
	}
	void SetOutFile(fstream* file);
	void SaveToFile();

private:
	void  StochasticDescentRigid();
	float GetDistModHausdorff(int camid);
	void SegmentGrayThreshold(CvMat* img, vector<int>& interiorIdx, int threshold, bool updateimg = false );
	void LinearPredictNextParams();

	int paramMode;

	bool bIsInit;
	bool bIsConverged;		   // have we converged on this iteration?
	int numcams;
	fstream* fileout;

	vector< vector<int>  > pixelIdxReal;
	vector< vector<int>  > pixelIdxGen;
	vector< vector<int>  > pixelIdxGenPrev;
	vector< vector<int>  > pixelIdxGenIn ;
	vector< vector<int>  > pixelIdxGenOut;

	vector<int> segPixCount;


	vector<CvMat*> genViews;   // what we render based on tracker parameters
	vector<CvMat*> realViews;  // the true views given

	// reference truth
	valarray<float>  truT; 
	valarray<float>  truR; 
	valarray<float>  truAng; 

	// previous time step best params
	valarray<float>  prevT; 
	valarray<float>  prevR; 
	valarray<float>  prevAng; 

	// current best parameters, revert to these if a sample fails to improve score
	valarray<float>  curT; 
	valarray<float>  curR; 
	valarray<float>  curAng;

	// predicted params, what we return to the renderer
	valarray<float>  predT; 
	valarray<float>  predR; 
	valarray<float>  predAng; 

	// initial predicted params, either equal to previous step 
	// or predicted from dynamics (e.g. first order linear)
	valarray<float>  init_predT; 
	valarray<float>  init_predR; 
	valarray<float>  init_predAng; 

	int steps;    // timesteps tracked
	int attempts; // attempts at each steps

	float distBest;
	vector<float> distBestUnNormalized;
	float distCur;
	vector<float> distCurUnNormalized;

};